#pragma once

#include "cartographer/rcm/commmon.h"
#include "cartographer/sensor/imu_data.h"

namespace cartographer
{
    const float GRAVITY_VALUE = 9.81;

    typedef std::vector<sensor::ImuData> ImuSet;

    // IMU biases (gyro and accelerometer)
    /*
    IMU的确定性误差厂家已经标定好，应用时得到的加速度和角速度值只包含随机性误差，随机误差需要实时估计。
    因此随机性误差可归为两类：
        1）高斯白噪声（误差）：测量噪声是AD转换器件引起的外部噪声，波动激烈的测量白噪声；
        2）随机游走（零偏Bias）：随机游走噪声是传感器内部机械、温度等各种物理因素产生的传感器内部误差的综合参数，是变换缓慢的bias。
    总bias = 零偏重复性产生的bias值 + bias instability（零偏不稳定性误差，不确定性零偏）
    */
    class ImuBias
    {
    public:
        ImuBias()
            : bax(0), bay(0), baz(0), bwx(0), bwy(0), bwz(0)
        {
        }

        ImuBias(const Eigen::Vector3d &acc_bias, const Eigen::Vector3d &gyro_bias)
            : bax(acc_bias(0)), bay(acc_bias(0)), baz(acc_bias(0)), bwx(gyro_bias(0)), bwy(gyro_bias(1)), bwz(gyro_bias(2))
        {
        }

        ImuBias(const float &b_acc_x, const float &b_acc_y, const float &b_acc_z,
                const float &b_ang_vel_x, const float &b_ang_vel_y, const float &b_ang_vel_z)
            : bax(b_acc_x), bay(b_acc_y), baz(b_acc_z), bwx(b_ang_vel_x), bwy(b_ang_vel_y), bwz(b_ang_vel_z)
        {
        }

    public:
        float bax, bay, baz;
        float bwx, bwy, bwz;
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };

    struct ImuDataAlias
    {
        common::Time time;
        Eigen::Vector3d a = Eigen::Vector3d::Zero();
        Eigen::Vector3d w = Eigen::Vector3d::Zero();

        ImuDataAlias()
        {
            a = Eigen::Vector3d::Zero();
            w = Eigen::Vector3d::Zero();
        }

        ImuDataAlias(const sensor::ImuData &data)
        {
            a = data.linear_acceleration;
            w = data.angular_velocity;
            time = data.time;
        }
        ImuDataAlias(const ImuDataAlias &data)
        {
            a = data.a;
            w = data.w;
            time = data.time;
        }
    };

    inline bool HasNan(const sensor::ImuData &data)
    {
        return HasNan(data.linear_acceleration) || HasNan(data.angular_velocity);
    }

    inline void Print(const sensor::ImuData &data)
    {
        LOG(INFO) << "imu " << common::ToUniversal(data.time)
                  << " ang: " << data.angular_velocity(0) << "," << data.angular_velocity(1) << "," << data.angular_velocity(2)
                  << " acc: " << data.linear_acceleration(0) << "," << data.linear_acceleration(1) << "," << data.linear_acceleration(2)
                  << "\n\n";
    }

    template<typename Type>
    Eigen::Matrix<Type, 3, 3> NormalizeRotation(const Eigen::Matrix<Type, 3, 3> &R)
    {
        Eigen::JacobiSVD<Eigen::Matrix<Type, 3, 3>> svd(R, Eigen::ComputeFullU | Eigen::ComputeFullV);
        return svd.matrixU() * svd.matrixV().transpose();
    }

    int CollectImuByTimeSpan(const std::deque<sensor::ImuData> &imu_data,
                             std::vector<sensor::ImuData> &imu_set,
                             const common::Time &time_start,
                             const common::Time &time_end,
                             const double timeOutThresh = 0.03);
} // namespace cartographer